#pragma once
#include "FastSPI_LED2.h"

template <uint8_t DATA_PIN1,uint8_t DATA_PIN2>
class CustomWS2811Controller : public WS2811Controller800Mhz<DATA_PIN1>
{
	typedef typename FastPin<DATA_PIN1>::port_ptr_t data_ptr_t;
	typedef typename FastPin<DATA_PIN1>::port_t data_t;

	uint8_t mPinMask2;
public:

	virtual void init() {
		WS2811Controller800Mhz<DATA_PIN1>::init();
		pinMode(DATA_PIN2, OUTPUT);
		WS2811Controller800Mhz<DATA_PIN2> pin2;
		mPinMask2 = pin2.mPinMask;
	}

	virtual void showRGB(register uint8_t *data, register int nLeds) {
		cli();
		
		register data_t mask = WS2811Controller800Mhz<DATA_PIN1>::mPinMask;
		register data_t mask2 = 4;
		register data_ptr_t port = WS2811Controller800Mhz<DATA_PIN1>::mPort;
		nLeds /= 2;
		nLeds *= (3);
		register uint8_t *end = data + nLeds; 
		register data_t hi_hi = *port | mask | mask2;
		register data_t hi_lo = (*port | mask) & ~mask2;
		register data_t lo_hi = (*port & ~mask) | mask2;
		register data_t lo_lo = (*port & ~mask) & ~mask2;
		*port = lo_lo;
		register uint8_t *data2 = end;

		register uint32_t b1 = *data++;
		register uint32_t b2 = *data2++;
		while(data <= end) { 
			// TODO: hand rig asm version of this method.  The timings are based on adjusting/studying GCC compiler ouptut.  This
			// will bite me in the ass at some point, I know it.
			for(register uint32_t i = 7; i > 0; i--) { 
				FastPin<DATA_PIN2>::fastset(port, hi_hi);
				delaycycles<NS(320) - 5>(); // 3 cycles - 1 store, 1 test, 1 if
				if(b1 & 0x80) { 
					if(b2 & 0x80) {FastPin<DATA_PIN2>::fastset(port, hi_hi);}
					else { FastPin<DATA_PIN2>::fastset(port, hi_lo); }
				} 
				else { 					
					if(b2 & 0x80) {FastPin<DATA_PIN2>::fastset(port, lo_hi);}
					else { FastPin<DATA_PIN2>::fastset(port, lo_lo); }
				}
				b1 <<= 1;
				b2 <<= 1;
				delaycycles<NS(320) - 5>(); // 3 cycles, 1 store, 1 store/skip,  1 shift 
				FastPin<DATA_PIN2>::fastset(port, lo_lo);
				delaycycles<NS(550) - 3>(); // 3 cycles, 1 store, 1 sub, 1 branch backwards
			}
			// extra delay because branch is faster falling through
			delaycycles<1>();

			// 8th bit, interleave loading rest of data
			FastPin<DATA_PIN2>::fastset(port, hi_hi);
			delaycycles<NS(320) - 5>();
			if(b1 & 0x80) { 
				if(b2 & 0x80) {FastPin<DATA_PIN2>::fastset(port, hi_hi);}
				else { FastPin<DATA_PIN2>::fastset(port, hi_lo); }
			} 
			else { 					
				if(b2 & 0x80) {FastPin<DATA_PIN2>::fastset(port, lo_hi);}
				else { FastPin<DATA_PIN2>::fastset(port, lo_lo); }
			}
			delaycycles<NS(320) - 3>(); // 4 cycles, 2 store, store/skip
			FastPin<DATA_PIN2>::fastset(port, lo_lo);
			b1 = *data++;
			b2 = *data2++;
			delaycycles<NS(550) - 7>(); // 1 store, 2 load, 1 cmp, 1 branch backwards, 1 movim
		};

		sei();
	}};

